function [x_opt,a_opt] = optimization_ntorso(LineNumber)
global ns_time s_time ns_slope s_knee ns_knee hip_pos R1

OutPutType = num2str(LineNumber);
% load('IndexAll');
% % indexAll = [1;1;1];

indexHippos = 2;%IndexAll(LineNumber,1)
indexSlope =3; 
% indexKnee = 2;%IndexAll(LineNumber,2)
%%%%%%%%%%%%%Add paths to mathematica compiled files:

addpath('./model/build_ntorso')
addpath('./model/buildopt_ntorso')
addpath('./wrappers_ntorso')
% addpath('./Data')

% addpath('/home/shu/Documents/Research_Related/Data/meanValue')
addpath('./data')
%  comment by Shu
% %%%%%%%%%%%%%Loading the data:
%

% %%%%%%%%%%%%%Outputs from loading the data:
switch indexHippos
    case 1
        load('mean_data_hippos.mat')
        hip_pos = hip_pos;
    case 2
        load('mean_data_com.mat')
        hip_pos = COMpos;
end

switch indexSlope
    case 1
        load('mean_data_nsslope.mat')
        ns_slope = ns_slope;
    case 2
        load('mean_data_HipV.mat')
        ns_slope = hipV;
    case 3
        load('mean_data_nsCOM.mat')
        ns_slope = ns_COM;
    case 4
        load('mean_data_COMangle.mat')
        ns_slope = COMangle;
end
% 
% switch indexKnee
%     case 1
        load('mean_data_sknee.mat')
        load('mean_data_nsknee.mat')
% %     case 2
%         1
%         load('mean_data_stHipLen.mat')
%         s_knee = st_HipLen;
%         load('mean_data_nsHipLen.mat')
%         ns_knee = ns_HipLen;
% %     case 3
%         load('mean_data_stCOMLen.mat')
%         s_knee = st_COMLen;
%         load('mean_data_nsCOMLen.mat')
%         ns_knee = ns_COMLen;
% % end
% %%%





ns_time = ns_time;%*0.5208/0.5718;
s_time = ns_time;
%%%%%%%%%%%%%Initial fit parameters (as determined by mathemaitca right now):
%%%%%%%%%%%%%% TODO:  determine the initial parameters from the data in
%%%%%%%%%%%%%% matlab

% a0 = [1.177         0         0         0         0;
%  0.204542, 7.93792,  0.275928, -0.685374, 0.182588; % swing leg slope
% -0.138531,13.9634,  0.152133, 3.18634, 0.29844;
% -0.353784,-11.0376,-0.187248,-0.710308,0.701144];

load('Afit.mat', 'a0');
a0
hip_pos = hip_pos-a0(1,5);

% a0(1,1) = 1.17;
% hip_pos = a0(1,1)*ns_time;

%%%%%%%%%%%%%% Minimization to verify optimality of initial parameters w.r.t the HBC
% options = optimset ('Display','iter','MaxFunEvals',6000);
% [a_fit,cost_fit] = fminunc(@cost,flatten(a0),options)


%%%%%%%%%%%%%% In the case that you don't want to run the optimizatino to
%%%%%%%%%%%%%% verify the parameters, uncomment this:
a0(1,5) =0;
a_fit = flatten(a0);

%%%%%%%%%%%%%% Constrain the parameters of the hip position to be zero
ubmat = Inf*ones(4,5);
lbmat = -Inf*ones(4,5);
% lbmat(1,1) = 0.8;
ubmat(1,2) = 0;
ubmat(1,3) = 0; ubmat(1,4) = 0; ubmat(1,5) = 0;%a0(1,5);
lbmat(1,2) = 0;
lbmat(1,3) = 0; lbmat(1,4) = 0; lbmat(1,5) = 0;%a0(1,5);
ub = flatten(ubmat);
lb = flatten(lbmat);


%%%%%%%%%%%%%% Other conditions for the optimization:
Aeq = [];
beq = [];
A = [];
b = [];

%%%%%%%%%%%%%% Optimizing to achieve HZD with the HBC:

%%%%% Conditions for optimization: tight error
errorbound = 1e-5;
options = optimset('Algorithm','sqp',...
    'Display','iter','TolX',errorbound,...
    'MaxFunEvals',8000,'TolCon',errorbound);

%%%%% Conditions for optimization:
% options = optimset ('Display','iter','MaxFunEvals',6000);

%%%%% Optimization
tic
[a_out,cost_out] = fmincon(@cost,a_fit,A,b,Aeq,beq,lb,ub,@mycon,options);
toc

%%%%%%%%%%%%%% Output the parameter matrix and the fit matrix:
a_opt = unflatten(a_out)
a_fit = unflatten(a_fit)

%%%%%%%%%%%%%% Output the initial condition:
% q = theta_a(a_opt);
q = inverse_kin(a_opt)
qdot = H_minus(q,a_opt)\[a_opt(1,1); 0; 0; 0];
x_opt = [q; qdot]

%%%%%%%%%%%%%% Output the final time:
tau(x_opt,a_opt)

%%%%%%%%% criterion %%%%%%%%%%%%%
HM_hippos = yd_hip_pos(ns_time,a_opt);
R_hippos = corrcoef(hip_pos,HM_hippos(:));  % hip_position

HM_nsslope = yd_ns_slope(ns_time,a_opt);
R_nsslope = corrcoef(ns_slope,HM_nsslope(:)); % non-stance slope

HM_sknee = yd_s_knee(ns_time,a_opt);
R_sknee = corrcoef(s_knee, HM_sknee);% stance knee

HM_nsknee = yd_ns_knee(ns_time,a_opt);
R_nsknee = corrcoef(ns_knee, HM_nsknee);% non-stance knee


R_data = [R_hippos(2,1);R_nsslope(2,1);R_sknee(2,1);R_nsknee(2,1)];

Rname = strcat('R_data_',OutPutType,'.mat');
save(Rname,'R_data')


%%%%%%%%%%%%%% Save initial and final condition
save('x_opt.mat','x_opt')
save('a_opt.mat','a_opt') %%%%%%%%%%%%%%% commet by Shu


Aname = strcat(OutPutType,'a_opt.mat');
save(Aname,'a_opt');
Xname = strcat(OutPutType,'x_opt.mat');
save(Xname,'x_opt');
Costname = strcat(OutPutType,'cost.mat');
save(Costname,'cost_out');
% step(a_opt)
%%%%%%%%%%%%%% Plot the output vs. fit vs. the human data

fig10 = figure(10); clf;
set(gca,'FontSize',12);
apc = [0.100000000000000   0.2000000000000   0.775000000000000   0.690000000000000];
set(gca,'position',apc);
pp1 = plot(ns_time,yd_hip_pos(ns_time,a_opt),'k.',...
    ns_time,yd_hip_pos(ns_time,a_fit),...
    'r',ns_time,hip_pos,'b.');
set(pp1,'MarkerSize', 12,'LineWidth',3);
xlabel({'Time(s)'},'Interpreter','LaTex','FontSize',20)
ylabel({'Position(m)'},'Interpreter','LaTex','FontSize',20)
l=legend(gca,pp1,'Subject1',{'$COM_x^D$','$COM_x^F$','$COM_x^H$'},1, ...
    'Location', 'BestOutside', 'Orientation','horizontal','Interpreter','LaTeX','FontSize',20);
p = get(l,'position');
set(l, 'position', [0 p(2)-.2 1 p(4)], 'Box','off');
figname = strcat(OutPutType,'COMpos.eps');
print(gcf, '-depsc',figname)







fig11 = figure(11); clf;
% plot(ns_time,yd_ns_slope(ns_time,a_opt),'b',ns_time,yd_ns_slope(ns_time,a_fit),'r',ns_time,ns_slope,'k.')
set(gca,'FontSize',12);
apc = [0.100000000000000   0.2000000000000   0.775000000000000   0.690000000000000];
set(gca,'position',apc);
pp1 = plot(ns_time,yd_ns_slope(ns_time,a_opt),'k.',...
    ns_time,yd_ns_slope(ns_time,a_fit),...
    'r',ns_time,ns_slope,'b.');
set(pp1,'MarkerSize', 12,'LineWidth',3);
xlabel({'Time(s)'},'Interpreter','LaTex','FontSize',20)
ylabel({'Slope'},'Interpreter','LaTex','FontSize',20)
l=legend(gca,pp1,'Subject1',{'$nsCOM^D$','$nsCOM^F$','$nsCOM^H$'},1, ...
    'Location', 'BestOutside', 'Orientation','horizontal','Interpreter','LaTeX','FontSize',20);
p = get(l,'position');
set(l, 'position', [0 p(2)-.2 1 p(4)], 'Box','off');
figname = strcat(OutPutType,'nsslope.eps');
print(gcf, '-depsc',figname)



fig12 = figure(12); clf;
set(gca,'FontSize',12);
apc = [0.100000000000000   0.2000000000000   0.775000000000000   0.690000000000000];
set(gca,'position',apc);
pp1 = plot(ns_time,yd_s_knee(ns_time,a_opt),'k.',...
    ns_time,yd_s_knee(ns_time,a_fit),...
    'r',ns_time,s_knee,'b.');
set(pp1,'MarkerSize', 12,'LineWidth',3);
xlabel({'Time(s)'},'Interpreter','LaTex','FontSize',20)
ylabel({'Angle(rad)'},'Interpreter','LaTex','FontSize',20)
l=legend(gca,pp1,'Subject1',{'$Y^D$','$Y^F$','$Y^H$'},1, ...
    'Location', 'BestOutside', 'Orientation','horizontal','Interpreter','LaTeX','FontSize',20);
p = get(l,'position');
set(l, 'position', [0 p(2)-.2 1 p(4)], 'Box','off');
figname = strcat(OutPutType,'sknee.eps');
print(gcf, '-depsc',figname)


fig13 = figure(13); clf;
set(gca,'FontSize',12);
apc = [0.100000000000000   0.2000000000000   0.775000000000000   0.690000000000000];
set(gca,'position',apc);
pp1 = plot(ns_time,yd_ns_knee(ns_time,a_opt),'k.',...
    ns_time,yd_ns_knee(ns_time,a_fit),...
    'r',ns_time,ns_knee,'b.');
set(pp1,'MarkerSize', 12,'LineWidth',2);
xlabel({'Time(s)'},'Interpreter','LaTex','FontSize',20)
ylabel({'Angle(rad)'},'Interpreter','LaTex','FontSize',20)
l=legend(gca,pp1,'Subject1',{'$Y^D$','$Y^F$','$Y^H$'},1, ...
    'Location', 'BestOutside', 'Orientation','horizontal','Interpreter','LaTeX','FontSize',20);
p = get(l,'position');
set(l, 'position', [0 p(2)-.2 1 p(4)], 'Box','off');
figname = strcat(OutPutType,'nsknee.eps');
print(gcf, '-depsc',figname)




% saveas(fig10,figname)

% main_ntorso(OutPutType)
%%%%%%%%%%%%%% Run main or step
%  main_ntorso(OutPutType)
end



function ret = cost(aflat)
global ns_time s_time ns_slope s_knee ns_knee hip_pos

%%%%%%%%%%%%%% Unflatten the a matrix
a = unflatten(aflat);


cost_ns_slope = sum(abs(ns_slope-yd_ns_slope(ns_time,a)));
cost_hip_pos = sum(abs(hip_pos-yd_hip_pos(ns_time,a)));
cost_s_knee = sum(abs(s_knee-yd_s_knee(s_time,a)));
cost_ns_knee = sum(abs(ns_knee-yd_ns_knee(ns_time,a)));


%%%%%%%%%%%%%% Wieghting for the fits

beta_ns_slope = 1/(max(ns_slope)-min(ns_slope));
beta_hip_pos = 1/(max(hip_pos)-min(hip_pos));
beta_s_knee = 1/(max(s_knee)-min(s_knee));
beta_ns_knee = 1/(max(ns_knee)-min(ns_knee));
beta = beta_ns_slope+beta_hip_pos+beta_s_knee+beta_ns_knee;

cost_all = [
    beta_ns_slope*cost_ns_slope,...
    beta_hip_pos*cost_hip_pos,...
    beta_s_knee*cost_s_knee,...
    beta_ns_knee*cost_ns_knee
    ];

ret = sum(cost_all)/beta;

end


%%%%%%%%%%%%%% Constraints to enforce HZD (or partial HZD)

function [c,ceq] = mycon(aflat)
global R1 ns_time

%%%%%%%%%%%%%% Unflatten the a matrix
a = unflatten(aflat);

%%%%%%%%%%%%%% Determine the initial condition (on the guard) for the
%%%%%%%%%%%%%% current parameters
% q = theta_a(a);
q = inverse_kin(a);
% q = find_theta_a(a);
qdot = H_minus(q,a)\[a(1,1); 0; 0; 0];


x = [q; qdot];

ic = resetFunc(x);
p_hip_ic = phip_sca(ic);


% % % % % Tmin = fminbnd(@(t)height(t,a,p_hip_ic),0,tau(q,a));
% % % % % hmin = height(Tmin,a,p_hip_ic);

%%%%%%%%%%%%%% H is the post impact velocities.
H = H_plus(q,a)*P(x);

%%%%%%%%%%%%%% Require the post impact velocities be completlty determined:
%      Hzd = H - [a(1,1); 0; 0; 0];  % full
Hzd = H(2:4,:) - [0; 0; 0];  %%%%0824
%     Hfullzd = H(1,:)-a(1,1);
%      bound = .000;
%%%%%%%%%%%%%% Pre and Post Impact knee angles

%      R1 = [1 1 -1 -1;
%          0 0  0  1;
%          0 0 -1 0;
%          0 1  0 0];
R1 = [1 1 1 -1;
    0 0  0  1;
    0 0 -1 0;
    0 1  0 0];

qR1 = R1*q;

%%%%%%%%%%%%%% Nonlinear inequalities
%%%%%%%%%%%%%%%%% gaurd must be negative
%%%%%%%%%%%%%%%%% Time must be less than the human time
c = [
    h_dot_minus(q)*qdot+.01;
    % %            -q(4);
    % %            qdot(2)-20;
    % %            -qdot(2)-10; % -10<qdot(2)<20;
    % %            -qdot(3)-20;
    % %            qdot(3)-10; % -20<qdot(3)<10
    % %            -qdot(4)-10;
    % %            qdot(4);% -20<qdot(4)<10;
    ];
% % %     c = [
% % %            h_dot_minus(q)*qdot+.01;
% % %            qdot(2)-20;
% % % %            -qdot(2); %
% % % %            qdot(3); %
% % %            -qdot(3)-20;
% % % %            qdot(4); %
% % %            -qdot(4)-20;
% % %  %         tau(q,a)-0.5208;
% % % %            -hmin + 0.000;  %%% 0824
% % % %              -hmin-0.001; % SHU
% % % %              -hmin - 0.001%.000;%%%%%%%%%%%%%%%%%0811
% % % %           Hfullzd-bound;
% % % %          -Hfullzd+bound;
% % % %         -qR1(2,1)+.000;
% % % %         -q(2,1)+.000;
% % % %         -qR1(4,1)+.000;
% % % %         -q(4,1)+.000;
% % % %          q(4,1)-.2;
% % % %         qR1(4,1)-.2;
% % % %         qR1(2,1)-.2;
% % % %         q(2,1)-.2
% % %         ];

%%%%%%%%%%%%%% Nonlinear equalities
%%%%%%%%%%%%%%%%% y pre impact must be zero
%%%%%%%%%%%%%%%%% y dot post impact must be zero
%%%%%%%%%%%%%%%%% guard must be satisfied
ceq = [
    y_minus(q,a);
    tau(q,a)-ns_time(end,1);
    %        tau(q,a)-0.5208;
    %        p_hip_ic-a(1,5);
    Hzd];


end



%%%%%%%Computing the position at impact through inverse kinematics

function ret = inverse_kin(a)

typeofsolve = 1;


if typeofsolve == 1
    
    %%%%%%%%%%%%%%%%%%%%% Numeric inverse kinematics
    %%%%%q0 as computed from the human parameters
    load('x_ic.mat')
    ic = x_opt;
    % % % %     ic(3) = -ic(3);
    % % % %     q0 = ic(1:4,:);
    q0 = [   -0.5295
        0.2821
        0.6505%0.6505
        0.0059];
    %%%Numerically solving for the initial condition
    
    options = optimset('Display','off');
    qzero = fsolve(@(q)inverse_kin_fun(q,a),q0,options);
    % %%%%If you update the inital guess on each iteration, the optimization does
    % %%%%not converge
    % q0 = qzero
    ret = qzero;
    
elseif typeofsolve == 2
    %%%%If using the old version (no torso) use this:
    
    % theta_a(a)
    %
    
    
    %%%If using the new version with the torso, use this:
    %%%%(the calculation is broken up, saves a lot of computation time)
    
    q = zeros(5,1);
    q = theta_a(a);
    %     q(2) = theta_a2(q,a);
    % %     q(5) = theta_a5(q,a);
    %     q(1) = theta_a1(q,a);
    %     q(4) = theta_a4(q,a);
    %     q(3) = theta_a3(q,a);
    %      q(4) = theta_a4(q,a);
    
    ret = q;
end

end

function F = inverse_kin_fun(q,a)

phipcond = phip_sca(R(q));
y2plus = ya2_vec(R(q), phipcond, a)-yd2_vec(R(q), phipcond,  a);

F = [y2plus;
    h_sca(q)];

% F = [y_plus(q,a);
%      h_sca(q)];

end

function ret = R(q)

dimq = length(q);
x = [q; zeros(dimq,1)];
xplus = resetFunc(x);

ret = xplus(1:dimq);

end

function ret = P(x)

dimq = length(x)/2;
xplus = resetFunc(x);

ret = xplus(dimq+1:end);

end
%%%%%%%%%%%%%% Canonical human functions

function ret = yd_ns_slope(t,a)

ret =exp(-a(2,4)*t).*(a(2,1)*cos(a(2,2)*t)+a(2,3)*sin(a(2,2)*t))+a(2,5);

end

function ret = yd_hip_pos(t,a)

ret = a(1,1)*t;%+a(1,5);

end

function ret = yd_s_knee(t,a)

ret = exp(-a(3,4)*t).*(a(3,1)*cos(a(3,2)*t)+a(3,3)*sin(a(3,2)*t))+a(3,5);

end

function ret = yd_ns_knee(t,a)

ret = exp(-a(4,4)*t).*(a(4,1)*cos(a(4,2)*t)+a(4,3)*sin(a(4,2)*t))+a(4,5);

end

function aflat = flatten(a)
aflat = a(:);
end

function aunflat = unflatten(aflat)
aunflat = reshape(aflat,4,5);
end
